#include "motor.h"
#include "MK60_FTM.h"
#include "bin_include.h"

bool motor_on = false;

void MotorInit(void)
{
	ftm_pwm_init(MOTOR_FTM, MOTOR_A_H_CH, MOTOR_FREQ, 0);
	ftm_pwm_init(MOTOR_FTM, MOTOR_A_L_CH, MOTOR_FREQ, 0);

	ftm_pwm_init(MOTOR_FTM, MOTOR_B_H_CH, MOTOR_FREQ, 0);
	ftm_pwm_init(MOTOR_FTM, MOTOR_B_L_CH, MOTOR_FREQ, 0);

	ftm_pwm_duty(MOTOR_FTM, MOTOR_A_H_CH, 0);
	ftm_pwm_duty(MOTOR_FTM, MOTOR_A_L_CH, 0);
	ftm_pwm_duty(MOTOR_FTM, MOTOR_B_H_CH, 0);
	ftm_pwm_duty(MOTOR_FTM, MOTOR_B_L_CH, 0);

	gpio_init(MOTOR_A_EN, GPO, 1);
	gpio_init(MOTOR_B_EN, GPO, 1);
}

void MotorOut(int16 leftPWM, int16 rightPWM)
{
	if (leftPWM > 0)
	{
		ftm_pwm_duty(MOTOR_FTM, MOTOR_A_H_CH, leftPWM);
		ftm_pwm_duty(MOTOR_FTM, MOTOR_A_L_CH, 0);
	}
	else {
		ftm_pwm_duty(MOTOR_FTM, MOTOR_A_H_CH, 0);
		ftm_pwm_duty(MOTOR_FTM, MOTOR_A_L_CH, -leftPWM);
	}

	if (rightPWM > 0)
	{
		ftm_pwm_duty(MOTOR_FTM, MOTOR_B_H_CH, rightPWM);
		ftm_pwm_duty(MOTOR_FTM, MOTOR_B_L_CH, 0);
	}
	else {
		ftm_pwm_duty(MOTOR_FTM, MOTOR_B_H_CH, 0);
		ftm_pwm_duty(MOTOR_FTM, MOTOR_B_L_CH, -rightPWM);
	}
}


void EncoderInit(void)
{
	ftm_quad_init(FTM1);
	ftm_quad_init(FTM2);
}